#include "DJI_motor.h"

DJ_Motor Claw_Motor[5] = {0};    // 使用fdcan1
DJ_Motor Arm_Motor[5] = {0};     // 使用fdcan2
DJ_Motor Chassis_Motor[2] = {0}; // 使用fdcan3          1.2使用邮箱1
void DJI_Init()
{
    FDCAN_FilterTypeDef sFilterConfig;
    sFilterConfig.IdType = FDCAN_STANDARD_ID; // ???ID?????ID??????
    sFilterConfig.FilterIndex = 0;
    sFilterConfig.FilterType = FDCAN_FILTER_MASK;
    sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
    sFilterConfig.FilterID1 = 0x00000000; //
    sFilterConfig.FilterID2 = 0x00000000; //
    if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK)
    {
        Error_Handler();
    }

    if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, FDCAN_TX_BUFFER0) != HAL_OK)
    {
        Error_Handler();
    }
    /*---------------------------- */
    if (HAL_FDCAN_ConfigFilter(&hfdcan2, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan2, FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK)
    {
        Error_Handler();
    }
    if (HAL_FDCAN_ActivateNotification(&hfdcan2, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, FDCAN_TX_BUFFER1) != HAL_OK)
    {
        Error_Handler();
    }
    /*---------------------------- */
    if (HAL_FDCAN_ConfigFilter(&hfdcan3, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan3, FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK)
    {
        Error_Handler();
    }
    if (HAL_FDCAN_ActivateNotification(&hfdcan3, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, FDCAN_TX_BUFFER2) != HAL_OK)
    {
        Error_Handler();
    }
    HAL_Delay(500);
    HAL_FDCAN_Start(&hfdcan1);
    HAL_FDCAN_Start(&hfdcan2);
    HAL_FDCAN_Start(&hfdcan3);
}

void Claw_Send(void)
{
    FDCAN_TxHeaderTypeDef TxHeader;
    uint8_t data[8];
    data[0] = (uint8_t)(Claw_Motor[0].tx_data >> 8);
    data[1] = (uint8_t)(Claw_Motor[0].tx_data & 0x00ff);
    data[2] = (uint8_t)(Claw_Motor[1].tx_data >> 8);
    data[3] = (uint8_t)(Claw_Motor[1].tx_data & 0x00ff);
    data[4] = (uint8_t)(Claw_Motor[2].tx_data >> 8);
    data[5] = (uint8_t)(Claw_Motor[2].tx_data & 0x00ff);
    data[6] = (uint8_t)(Claw_Motor[3].tx_data >> 8);
    data[7] = (uint8_t)(Claw_Motor[3].tx_data & 0x00ff);
    TxHeader.Identifier = 0x200;
    TxHeader.IdType = FDCAN_STANDARD_ID;              // 标准ID
    TxHeader.TxFrameType = FDCAN_DATA_FRAME;          // 数据帧
    TxHeader.DataLength = 8 << 16;                    // 发送数据长度
    TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;  // 设置错误状态指示
    TxHeader.BitRateSwitch = FDCAN_BRS_OFF;           // 不开启可变波特率
    TxHeader.FDFormat = FDCAN_CLASSIC_CAN;            // 普通CAN格式
    TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // 用于发送事件FIFO控制, 不存储
    TxHeader.MessageMarker = 0x00;                    // 用于复制到TX EVENT FIFO的消息Maker来识别消息状态，范围0到0xFF
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, data);
		data[0] = (uint8_t)(Claw_Motor[4].tx_data >> 8);
		data[1] = (uint8_t)(Claw_Motor[4].tx_data & 0x00ff);
		data[2] = 0;
		data[3] = 0;
		data[4] = 0;
		data[5] = 0;
		data[6] = 0;
		data[7] = 0;
		TxHeader.Identifier = 0x1ff;
		HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, data);
    // return 1; // 发送
    // return 0;
}
void Arm_Send(void)
{
    FDCAN_TxHeaderTypeDef TxHeader;
    uint8_t data[8];
    data[0] = (uint8_t)(Arm_Motor[0].tx_data >> 8);
    data[1] = (uint8_t)(Arm_Motor[0].tx_data & 0x00ff);
    data[2] = (uint8_t)(Arm_Motor[1].tx_data >> 8);
    data[3] = (uint8_t)(Arm_Motor[1].tx_data & 0x00ff);
    data[4] = (uint8_t)(Arm_Motor[2].tx_data >> 8);
    data[5] = (uint8_t)(Arm_Motor[2].tx_data & 0x00ff);
    data[6] = (uint8_t)(Arm_Motor[3].tx_data >> 8);
    data[7] = (uint8_t)(Arm_Motor[3].tx_data & 0x00ff);
    TxHeader.Identifier = 0x200;
    TxHeader.IdType = FDCAN_STANDARD_ID;              // 标准ID
    TxHeader.TxFrameType = FDCAN_DATA_FRAME;          // 数据帧
    TxHeader.DataLength = 8 << 16;                    // 发送数据长度
    TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;  // 设置错误状态指示
    TxHeader.BitRateSwitch = FDCAN_BRS_OFF;           // 不开启可变波特率
    TxHeader.FDFormat = FDCAN_CLASSIC_CAN;            // 普通CAN格式
    TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // 用于发送事件FIFO控制, 不存储
    TxHeader.MessageMarker = 0x00;                    // 用于复制到TX EVENT FIFO的消息Maker来识别消息状态，范围0到0xFF
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan2, &TxHeader, data);
    data[0] = (uint8_t)(Arm_Motor[4].tx_data >> 8);
    data[1] = (uint8_t)(Arm_Motor[4].tx_data & 0x00ff);
    data[2] = 0;
    data[3] = 0;
    data[4] = 0;
    data[5] = 0;
    data[6] = 0;
    data[7] = 0;
    TxHeader.Identifier = 0x1ff;
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan2, &TxHeader, data);
}
void Chassis_Send(void)
{
    FDCAN_TxHeaderTypeDef TxHeader;
    uint8_t data[8];
    data[0] = (uint8_t)(Chassis_Motor[0].tx_data >> 8);
    data[1] = (uint8_t)(Chassis_Motor[0].tx_data & 0x00ff);
    data[2] = (uint8_t)(Chassis_Motor[1].tx_data >> 8);
    data[3] = (uint8_t)(Chassis_Motor[1].tx_data & 0x00ff);
    data[4] = 0;
    data[5] = 0;
    data[6] = 0;
    data[7] = 0;
    TxHeader.Identifier = 0x200;
    TxHeader.IdType = FDCAN_STANDARD_ID;              // 标准ID
    TxHeader.TxFrameType = FDCAN_DATA_FRAME;          // 数据帧
    TxHeader.DataLength = 8 << 16;                    // 发送数据长度
    TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;  // 设置错误状态指示
    TxHeader.BitRateSwitch = FDCAN_BRS_OFF;           // 不开启可变波特率
    TxHeader.FDFormat = FDCAN_CLASSIC_CAN;            // 普通CAN格式
    TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // 用于发送事件FIFO控制, 不存储
    TxHeader.MessageMarker = 0x00;                    // 用于复制到TX EVENT FIFO的消息Maker来识别消息状态，范围0到0xFF
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan3, &TxHeader, data);
}

// uint8_t DJI_CAN_SEND(DJ_Motor DJIM1, DJ_Motor DJIM2)
// {
//     FDCAN_TxHeaderTypeDef TxHeader;
//     uint8_t data[8];
//     data[0] = (uint8_t)(DJIM1.tx_data >> 8);
//     data[1] = (uint8_t)(DJIM1.tx_data & 0x00ff);
//     data[2] = (uint8_t)(DJIM2.tx_data >> 8);
//     data[3] = (uint8_t)(DJIM2.tx_data & 0x00ff);
//     data[4] = 0;
//     data[5] = 0;
//     data[6] = 0;
//     data[7] = 0;

//     TxHeader.Identifier = 0x200;
//     TxHeader.IdType = FDCAN_STANDARD_ID;              // 标准ID
//     TxHeader.TxFrameType = FDCAN_DATA_FRAME;          // 数据帧
//     TxHeader.DataLength = 8 << 16;                    // 发送数据长度
//     TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;  // 设置错误状态指示
//     TxHeader.BitRateSwitch = FDCAN_BRS_OFF;           // 不开启可变波特率
//     TxHeader.FDFormat = FDCAN_CLASSIC_CAN;            // 普通CAN格式
//     TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // 用于发送事件FIFO控制, 不存储
//     TxHeader.MessageMarker = 0x00;                    // 用于复制到TX EVENT FIFO的消息Maker来识别消息状态，范围0到0xFF

//     if (HAL_FDCAN_AddMessageToTxFifoQ(DJIM1.canhandle, &TxHeader, data) != HAL_OK)
//         return 1; // 发送
//     return 0;
// }